/*
 *  CollisionResolution.cpp
 *  c_Render
 *
 *  Created by liao peterson on 2/20/10.
 *  Copyright 2010 uestc. All rights reserved.
 *
 */
#include"Physics.h"

/*
 *Collision Resolver mainly consists of 3 stages:
 *1.calculate the velocity change of unit impulse.
 *2.calculate the desired velocity change.
 *3.apply the corresponding impulse to both objects.
 */

void applyVelChange(contact*temp,const vec3 impulseContact)
{
	impulse * ic = new impulse(-impulseContact, temp->relativeContactPosition[0]);
	impulse * ic2 = new impulse(impulseContact, temp->relativeContactPosition[1]);
	
	ic->f += (temp->one->pi.vel % temp->contactNormal) % temp->contactNormal * CONTACT_RESTITUITION * DURATION;
	temp->one->applyImpulse(ic, temp->velChange[0], temp->angVelChange[0]);
	
	ic2->f += (temp->two->pi.vel % temp->contactNormal) % temp->contactNormal * CONTACT_RESTITUITION * DURATION;
	temp->two->applyImpulse(ic2, temp->velChange[1], temp->angVelChange[1]);
}

void ResolveVel(contact*temp)
{
	if(temp->contactNormal.norm() < EPSILON)return;
	
	temp->calInternal();
	
	// Calculate the impulse needed, then apply the corresponding impulse the two objects respectively
	vec3 impulseContact = {temp->desiredDeltaVelocity / temp->deltaVelocity, 0, 0};
	impulseContact = temp->contact2World * impulseContact;
	
	applyVelChange(temp,impulseContact);
}

// For multi-contact resolution, resolve in the order of severity.
// For Velocity, severity means desired velocity change
void collisionManager::collisionResolveVel(collisionData*data)
{
	if(data->contacts->count==0)return;
	
	data->contacts->SetCurrent(0);
	GLfloat worstDeltaVel = 0.1f;
	contact*worst = NULL;
	contact*temp;
	for(int i=0;i<MAX_VELOCITY_ITERATIONS;i++)
	{
		data->contacts->SetCurrent(0);
		for(int j=0;j<data->contacts->count;j++)
		{
			temp = data->contacts->current->data;
			temp->calInternal();
			if(worstDeltaVel < temp->desiredDeltaVelocity)
			{
				worstDeltaVel = temp->desiredDeltaVelocity;
				worst = temp;
			}
			data->contacts->current = data->contacts->current->next;
		}
		
		if(worst==NULL)break;
		ResolveVel(worst);
		//if(data->contacts->count==1)break;
			
		// Adjust other desired delta velocity
		if(temp->one->pi.mass > STATIC_MASS)
		{
			data->contacts->SetCurrent(0);
			GLint tag = temp->one->identity;
			for(int i=0;i<data->contacts->count;i++)
			{
				temp = data->contacts->current->data;
				if(temp->one->identity == tag || temp->two->identity == tag)
					temp->calDesiredDeltaVel();
				data->contacts->current = data->contacts->current->next;
			}
		}
		
		if(temp->two->pi.mass > STATIC_MASS)
		{
			data->contacts->SetCurrent(0);
			GLint tag = temp->two->identity;
			for(int i=0;i<data->contacts->count;i++)
			{
				temp = data->contacts->current->data;
				if(temp->one->identity == tag || temp->two->identity == tag)
					temp->calDesiredDeltaVel();
				data->contacts->current = data->contacts->current->next;
			}
		}

		worstDeltaVel = 0.1f;
		worst = NULL;
	}
	
	/*if(data->contacts->count==0)return;
	
	data->contacts->SetCurrent(0);
	for(int i=0;i<data->contacts->count;i++)
	{
		ResolveVel(data->contacts->current->data);
		data->contacts->current=data->contacts->current->next;
	}*/
	
}

/**************************************************************************************
 **********ABOVE FOR VELOCITY**********************************************************
 **********BELOW FOR POSITION**********************************************************
 **********AND I'M THE LOVELY SEPERATOR:)**********************************************
 **************************************************************************************
*/

void applyPosChange(contact*temp, physicsIntegrator*one, const vec3 relativeContactPosition, GLfloat rotRes, GLfloat sum)
{
	Quat tquat;
	vec3 tAxis;
	GLfloat linear;
	
	tAxis = temp->contactNormal % relativeContactPosition;
	tAxis.normalize();
	linear = 1/one->pi.mass;
	rotRes /= sum;
	linear /= sum;
	rotRes /= (tAxis % relativeContactPosition) * temp->contactNormal;
	
	// Very Important line
	// Shrinking penetration prevent objects from vibrating between objects because of over position adujstment
	temp->penetration = temp->penetration * PENETRATION_SHRINKAGE;
	
	//printf("rotational resolution:%f\n",-rotRes * fabs(temp->penetration));
	//printf("axis:%f %f %f\n\n",tAxis.x,tAxis.y,tAxis.z);
	tquat.Set(tAxis, - rotRes * fabs(temp->penetration));
	
	/*rotRes *= fabs(temp->penetration);
	GLfloat a = (relativeContactPosition + temp->contactNormal * rotRes).norm();
	GLfloat b = relativeContactPosition.norm();
	GLfloat aux = (rotRes * rotRes - a*a - b*b)/(2*a*b);
	aux=MAX(-1,aux);
	aux=MIN(1,aux);
	GLfloat theta = acos(aux) - M_PI;
	
	tquat.Set(tAxis, -theta);*/
	
	one->pi.q =  tquat * one->pi.q;
	vec3 posChange = temp->contactNormal * linear * fabs(temp->penetration);
	//vec3 t=(temp->contactNormal % posChange) % temp->contactNormal * CONTACT_RESTITUITION;
	//posChange += (temp->contactNormal % posChange) % temp->contactNormal * CONTACT_RESTITUITION;
	one->pi.locOffset += posChange;
}

	

void ResolvePos(contact*temp)
{
	if(temp->contactNormal.norm() < EPSILON)return;
	
	vec3 relativeContactPosition[2];
	relativeContactPosition[0] = temp->contactPoint - temp->one->pi.loc;
	relativeContactPosition[1] = temp->contactPoint - temp->two->pi.loc;
	GLfloat sum = 0;
	GLfloat rotRes[2];
	
	if(temp->one->pi.mass>STATIC_MASS)
	{
		vec3 torquePerUnitImpulse = temp->contactNormal.cross(relativeContactPosition[0]);
		vec3 rotationPerUnitImpulse = torquePerUnitImpulse / temp->one->pi.uniMoment;
		vec3 velocityPerUnitImpulse = rotationPerUnitImpulse.cross(relativeContactPosition[0]);
		rotRes[0] = fabs(velocityPerUnitImpulse * temp->contactNormal);
		sum += 1/temp->one->pi.mass + rotRes[0];
	}
	
	if(temp->two->pi.mass>STATIC_MASS)
	{
		vec3 torquePerUnitImpulse = temp->contactNormal.cross(relativeContactPosition[1]);
		vec3 rotationPerUnitImpulse = torquePerUnitImpulse / temp->two->pi.uniMoment;
		vec3 velocityPerUnitImpulse = rotationPerUnitImpulse.cross(relativeContactPosition[1]);
		rotRes[1] = fabs(velocityPerUnitImpulse * temp->contactNormal);
		sum += 1/temp->two->pi.mass + rotRes[1];
	}
	
	if(temp->one->pi.mass>STATIC_MASS)
	{
		temp->contactNormal *= -1;
		applyPosChange(temp,temp->one,relativeContactPosition[0],rotRes[0],sum);
		temp->contactNormal *= -1;
	}
	
	if(temp->two->pi.mass>STATIC_MASS)
		applyPosChange(temp,temp->two,relativeContactPosition[1],rotRes[1],sum);
}


// For multi-contact resolution, resolve in the order of severity.
// For position, severity means interpenetration depth.
void collisionManager::collisionResolvePos(collisionData*data)
{
	if(data->contacts->count==0)return;
	
	GLfloat worstPen = -0.001f;
	contact*worst = NULL;
	for(int i=0;i<MAX_POSITION_ITERATIONS;i++){
		data->contacts->SetCurrent(0);
		for(int j=0;j<data->contacts->count;j++){
			if(data->contacts->current->data->estimatedPenetration>DEPARTURE_DIST)
			{
				data->contacts->current->data->penetration=BIG_VALUE;
				continue;
			}
			if(data->contacts->current->data->penetration<worstPen){
				worstPen = data->contacts->current->data->penetration;
				worst = data->contacts->current->data;
			}
			data->contacts->current = data->contacts->current->next;
		}
		if(worst == NULL)break;
		//worst->recalPen();
		ResolvePos(worst);
		//if(data->contacts->count==1)break;
		if(worst->one->pi.mass > STATIC_MASS)
		{
			worst->one->setMatrix();
			GLint tag = worst->one->identity;
			data->contacts->SetCurrent(0);
			for(int i=0;i<data->contacts->count;i++)
			{
				if(data->contacts->current->data->one->identity == tag || 
				   data->contacts->current->data->two->identity == tag)
					data->contacts->current->data->recalPen();
				data->contacts->current = data->contacts->current->next;
			}
		}
		if(worst->two->pi.mass > STATIC_MASS)
		{
			worst->two->setMatrix();
			GLint tag = worst->two->identity;
			data->contacts->SetCurrent(0);
			for(int i=0;i<data->contacts->count;i++)
			{
				if(data->contacts->current->data->one->identity == tag || 
				   data->contacts->current->data->two->identity == tag)
					data->contacts->current->data->recalPen();
				data->contacts->current = data->contacts->current->next;
			}
		}
		worstPen = -0.001f;
		worst = NULL;
	}
}


/**************************************************************************************
 **********AND I'M THE LOVELY SEPERATOR AGAIN:)****************************************
 **************************************************************************************
 */

void collisionManager::updateCache(collisionData*data)
{
	cache->contacts->SetCurrent(0);
	GLint breakFlag = 0;
	Node<contact>*cacheCurrent = cache->contacts->current;
	
	// Little trivial stuff:
	// in the for loop cache->contacts->count got changed
	// so keep a record of it first
	GLint cacheCount = cache->contacts->count;
	for(int i=0;i<cacheCount;i++)
	{
		if(!(cacheCurrent->data->one&&cacheCurrent->data->two)||
			(cacheCurrent->data->one->deleted||cacheCurrent->data->two->deleted))
		{
			cache->contacts->Dismiss(-1);
			cacheCurrent = cacheCurrent->next;
			continue;
		}
		
		data->contacts->SetCurrent(0);
		Node<contact>*dataCurrent = data->contacts->current;
		
		
		// Little trivial stuff:
		// in the for loop data->contacts->count got changed
		// so keep a record of it first
		GLint count = data->contacts->count;
		for(int j=0;j<count;j++)
		{
			if(cacheCurrent->data->Compare(dataCurrent->data))
			{
				cacheCurrent->data->copyVecs(dataCurrent->data);
				cacheCurrent->data->lifeCycle = 0;
				dataCurrent = dataCurrent->next;
				data->contacts->Dismiss(-1);
				breakFlag = 1;
				cache->contacts->IncCurrent(); 
				cacheCurrent = cacheCurrent->next;
				break;
			}else{
				data->contacts->IncCurrent();
				dataCurrent = dataCurrent->next;
			}
		}
		// current contact in cache missing in data
		// Update its parameters mannually.
		if(breakFlag == 0)
		{
			cacheCurrent->data->lifeCycle++;
			cacheCurrent->data->recalPen();
			if(cacheCurrent->data->estimatedPenetration > DEPARTURE_DIST || 
				cacheCurrent->data->lifeCycle >= MAX_CONTACT_LIFE)
			//if(cacheCurrent->data->penetration > DEPARTURE_DIST)
			{
				cacheCurrent = cacheCurrent->next;
				cache->contacts->Dismiss(-1);
			}
			else
			{
				cache->contacts->IncCurrent();
				cacheCurrent = cacheCurrent->next;
			}
		}
		
		breakFlag = 0;
	}
	
	// After looking for similar contact then append the new-comers in data to cache
	data->contacts->SetCurrent(0);
	for(int i=0;i<data->contacts->count;i++)
	{
		data->contacts->current->data->lifeCycle = 0;
		cache->contacts->Append(data->contacts->current->data);
		data->contacts->current = data->contacts->current->next;
	}
	
	/*if(cache->contacts->count!=0)
	{
		printf("cache counts:%d\n",cache->contacts->count);
		cache->contacts->SetCurrent(0);
		Node<contact>*n=cache->contacts->current;
		for(int i=0;i<cache->contacts->count;i++)
		{
			printf("One:%d Two:%d IndexOne:%d IndexTwo:%d\n",n->data->one->identity,
															n->data->two->identity,
															n->data->indexOne,
															n->data->indexTwo);
			printf("Contact point:%f %f %f\n",n->data->contactPoint.x,
												n->data->contactPoint.y,
												n->data->contactPoint.z);
			printf("Penetration:%f Orig:%f Estd:%f\n",n->data->penetration,
				   n->data->originalPenetration,
				   n->data->estimatedPenetration);
			printf("Contact Normal:%f %f %f\n",n->data->contactNormal.x,
				   n->data->contactNormal.y,
				   n->data->contactNormal.z);
			n=n->next;
		}
		printf("\n");
	}*/
}



